/**
 * Copyright (C) 2011  E-Robot
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>
 *
 * @brief Sonar.cpp
 *
 * @date 20 oct. 2010
 * @author Elias Capito
 * @version 0.1
 */

#include "Sonar.h"

Sonar::Sonar(int _index,  player_pose3d_t _pos,PositionEnum _posType)
{
	m_pos.setX(_pos.px);
	m_pos.setY(_pos.py);
	m_angle = _pos.pyaw;
	m_hyp = sqrt(_pos.px*_pos.px+_pos.py*_pos.py);
	m_index = _index;
	m_posType  = _posType;
	m_distance = 0;
	m_prox = AnyProx;
}

double Sonar::distancRobotCenterToCapteur(){
	return m_hyp;
}
QVector2D Sonar::vectorRobotCenterToCapteur(){
	return m_pos;
}

double Sonar::angle(){
	return m_angle;
}
double Sonar::distance() const
{
    return m_distance;
}

int Sonar::index() const
{
    return m_index;
}

PositionEnum Sonar::pos() const
{
    return m_posType;
}

void Sonar::setDistance(double _distance)
{
    this->m_distance = _distance;

    //Change la proximité du capteur
    if(_distance>=3){
		m_prox = AnyProx;
	}else if(_distance>=2.2){
		m_prox = ObjectDetect;
	}else if(_distance>=1.5){
		m_prox = Near;
	}else if(_distance>=0.7){
		m_prox = Nearly;
	}else if(_distance>=0){
		m_prox = Danger;
	}
}

void Sonar::setIndex(int m_index)
{
    this->m_index = m_index;
}

void Sonar::setPosType(PositionEnum m_pos)
{
    this->m_posType = m_pos;
}

SonarProximityEnum Sonar::prox() const{
	return m_prox;
}
void Sonar::defineVecteurToAlgo(AlgoEnum _algo, Vecteur _vec){
	m_hash[_algo] = _vec;
}
void Sonar::deleteVecteurToAlgo(AlgoEnum _algo){
	m_hash.erase(_algo);
}
Vecteur Sonar::getVecteurToAlgo(AlgoEnum _algo){
	return m_hash[_algo];
}

Sonar::~Sonar()
{
}
